#include "lasershow.hpp"
#include "csvFileRead.h"
#include "opencv2/imgproc/imgproc.hpp"


void LaserShow::setImageWdith(unsigned int Wdith)
{
    ImageWdith=Wdith;
}
void LaserShow::setImageHeight(unsigned int Height)
{
    ImageHeight = Height;
}
void LaserShow::setWindowName(string name)
{
    WindowName = name;
}
void LaserShow::laserShowInit(unsigned int Wdith,unsigned int Height,string Window_Name)
{
    setImageWdith(Wdith);
    setImageHeight(Height);
    setWindowName(Window_Name);
    namedWindow(WindowName);
    Image= Mat(Wdith,Height,CV_8UC3,Scalar(255,255,255)); 
}
void LaserShow::lasershow(Mat image)
{
   
    imshow(WindowName,image);
}
bool LaserShow::laserDataReadFromCSVfile(string filename)
{
    //read laser date from files
    csvFileRead read;
    read.read_csv_file(filename,laser_data);
}
void LaserShow::drawLaserImage(Mat *ImageCopy,LaserMessage laserMsg)
{
    //using laser date draw image
    int point_x,point_y;
    Image.copyTo(*ImageCopy);
    //draw the center of the circle
    Point laserPointXY(0,0);
    Point LaserCenter(ImageWdith/2,ImageHeight/2);

    circle(*ImageCopy,LaserCenter,3,Scalar(0,0,0),-1);
    //calculate point x y
    vector<float> laserData=laserMsg.ranges;
    vector<float> laserAngle=laserMsg.angles;
    int halfWidth  = ImageWdith/2;
    int halfHeight = ImageHeight/2;
    float angle=0;
    for(int it=0; it<laserData.size(); it++)
    {
        laserPointXY.x=(int)(laserData[it]*cos(laserAngle[it])*100)+halfWidth;
        laserPointXY.y=(int)(laserData[it]*sin(laserAngle[it])*100)+halfHeight;
        circle(*ImageCopy,laserPointXY,0.1,Scalar(0,0,0),-1);
    }
}

